{# ----------------------------------------------------------------------------
 # SymForce - Copyright 2022, Skydio, Inc.
 # This source code is under the Apache 2.0 license found in the LICENSE file.
 # ---------------------------------------------------------------------------- #}

{%- import "../../util/util.jinja" as util with context -%}

#include <algorithm>
#include <cassert>
#include <Eigen/Dense>

#include <sym/{{ camelcase_to_snakecase(cls.__name__) }}.h>

#include "./storage_ops.h"

namespace sym {

template <typename ScalarType>
void StorageOps<{{ cls.__name__ }}<ScalarType>>::ToStorage(const {{ cls.__name__ }}<ScalarType>& a, ScalarType* out) {
  assert(out != nullptr);
  std::copy_n(a.Data().data(), a.StorageDim(), out);
}

template <typename ScalarType>
{{ cls.__name__ }}<ScalarType> StorageOps<{{ cls.__name__ }}<ScalarType>>::FromStorage(const ScalarType* data) {
  assert(data != nullptr);
  return {{ cls.__name__ }}<ScalarType>(
    Eigen::Map<const typename {{ cls.__name__ }}<ScalarType>::DataVec>(data));
}

}  // namespace sym

// Explicit instantiation
{% for Scalar in scalar_types %}
template struct sym::StorageOps<sym::{{ cls.__name__ }}<{{ Scalar }}>>;
{% endfor %}
